int motor_left[] = {10, 11};
int motor_right[] = {5, 6};

int command = 0;              // incoming serial byte
int velocity_motor_left[2];
int velocity_motor_right[2];
int speed = 0;
int direction = 0;

int SPEED_0 = 0;
int SPEED_1 = 180;
int SPEED_2 = 210;
int SPEED_3 = 255;

void setup()
{
  Serial.begin(9600);         // start serial port at 9600 bps
  establishContact();
}

void establishContact() {
  while (Serial.available() <= 0) {
    delay(300);
  }
}

void loop()
{
  if (Serial.available() > 0) {
    command = Serial.read();
	  
    if (command == 'S') {
      setSpeed();
      setMotorVelocity(direction, speed);
    }
  }
}

void setSpeed() {
  waitSerial();
  
  command = Serial.read();
  
  if (command == '0') {
    speed = SPEED_0;
    Serial.println(speed);
  }
  else if (command == '1') {
    speed = SPEED_1;
    Serial.println(speed);
  }
  else if (command == '2') {
    speed = SPEED_2;
    Serial.println(speed);
  }
  else if (command == '3') {
    speed = SPEED_3;
    Serial.println(speed);
  }
}

void waitSerial() {
  while (Serial.available() <= 0) {
    delay(100);
  }
}

void setMotorVelocity(int paramDirection, int paramSpeed) {
  if (paramDirection == 0) {
    velocity_motor_left[0] = paramSpeed;
    velocity_motor_left[1] = 0;
    
    velocity_motor_right[0] = paramSpeed;
    velocity_motor_right[1] = 0;
  }

  analogWrite(motor_left[0], velocity_motor_left[0]);
  analogWrite(motor_left[1], velocity_motor_left[1]);
  
  analogWrite(motor_right[0], velocity_motor_right[0]);
  analogWrite(motor_right[1], velocity_motor_right[1]);
}
